#ifndef __MPU6050_H
#define __MPU6050_H

#include "wire.h"
#include <stdint.h>

// MPU6050 默认 I2C 地址（AD0 接 GND 时）
#define MPU6050_ADDR         0x68
#define MPU6050_ADDR_ALT     0x69  // AD0 接 VCC

// MPU6050 寄存器地址
#define MPU6050_REG_WHOAMI     0x75
#define MPU6050_REG_PWR_MGMT_1 0x6B
#define MPU6050_REG_CONFIG     0x1A
#define MPU6050_REG_GYRO_CONFIG 0x1B
#define MPU6050_REG_ACCEL_CONFIG 0x1C
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_TEMP_OUT_H 0x41
#define MPU6050_REG_USER_CTRL  0x6A
#define MPU6050_REG_INT_PIN_CFG 0x37
#define MPU6050_REG_INT_ENABLE 0x38

// 量程定义
#define MPU6050_GYRO_FS_250DPS   0x00
#define MPU6050_GYRO_FS_500DPS   0x01
#define MPU6050_GYRO_FS_1000DPS  0x02
#define MPU6050_GYRO_FS_2000DPS  0x03

#define MPU6050_ACCEL_FS_2G      0x00
#define MPU6050_ACCEL_FS_4G      0x01
#define MPU6050_ACCEL_FS_8G      0x02
#define MPU6050_ACCEL_FS_16G     0x03

// 温度转换
#define MPU6050_TEMP_OFFSET      36.53f
#define MPU6050_TEMP_SENSITIVITY 340.0f

// 数据结构
typedef struct {
    Wire* wire;
    uint8_t addr;

    // 原始数据
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
    int16_t temp;

    // 量程
    uint8_t gyro_fs;
    uint8_t accel_fs;

    // 校准偏移（原始 LSB）
    int16_t accel_offset_x;
    int16_t accel_offset_y;
    int16_t accel_offset_z;
    int16_t gyro_offset_x;
    int16_t gyro_offset_y;
    int16_t gyro_offset_z;

    // 是否已校准
    uint8_t calibrated;

} MPU6050;

// 函数声明
int MPU6050_init(MPU6050* mpu, Wire* wire, uint8_t address);
int MPU6050_begin(MPU6050* mpu, Wire* wire, uint8_t address);
int MPU6050_readRaw(MPU6050* mpu); //这个函数会从mpu6050读取最新的数据
int MPU6050_readTemp(MPU6050* mpu);
int MPU6050_setGyroFSScale(MPU6050* mpu, uint8_t fs_sel);
int MPU6050_setAccelFSScale(MPU6050* mpu, uint8_t fs_sel);

// 获取物理值（带单位）这些函数不会发起读mpu6050，而是读取本地缓存
float MPU6050_getAx_g(MPU6050* mpu);
float MPU6050_getAy_g(MPU6050* mpu);
float MPU6050_getAz_g(MPU6050* mpu);
float MPU6050_getGx_dps(MPU6050* mpu);
float MPU6050_getGy_dps(MPU6050* mpu);
float MPU6050_getGz_dps(MPU6050* mpu);
float MPU6050_getTemp_C(MPU6050* mpu);

// 校准函数
int MPU6050_calibrateAccel(MPU6050* mpu);
int MPU6050_calibrateGyro(MPU6050* mpu);

// 一次性获取所有物理值（带校准）
int MPU6050_getMotion(
    MPU6050* mpu,
    float* ax_g, float* ay_g, float* az_g,
    float* gx_dps, float* gy_dps, float* gz_dps,
    float* temp_c
);

// 重置校准
void MPU6050_resetCalibration(MPU6050* mpu);

// 获取总加速度（用于验证校准）
float MPU6050_getTotalAccel(MPU6050* mpu);

// 内部辅助函数
uint8_t MPU6050_readReg(MPU6050* mpu, uint8_t reg);
int MPU6050_writeReg(MPU6050* mpu, uint8_t reg, uint8_t data);

#endif
/*
Wire_begin(&wire, I2C1, 50000);
    MPU6050_begin(&mpu, &wire, MPU6050_ADDR);
	
	uint8_t who;
    if (Wire_readReg(&wire, MPU6050_ADDR, 0x75, &who) == 0) {
        printf("WHO_AM_I = 0x%02X\r\n", who);
    } else {
        printf("Read WHO_AM_I failed!\r\n");
    }
    // 设置量程
    MPU6050_setAccelFSScale(&mpu, MPU6050_ACCEL_FS_4G);
    MPU6050_setGyroFSScale(&mpu, MPU6050_GYRO_FS_500DPS);

    // ===== 校准（只运行一次）=====
    printf("cali...\n");
    delay_ms(2000);

    if (MPU6050_calibrateAccel(&mpu) != 0) {
        printf("acc cali failed\n");
    }
    if (MPU6050_calibrateGyro(&mpu) != 0) {
        printf("gyro cali failed\n");
    }

    printf("ready\n");

    // ===== 主循环 =====
    while (1) {
        float ax, ay, az, gx, gy, gz, temp;
		printf("%d\n",micros()/1000);
        if (MPU6050_getMotion(&mpu, &ax, &ay, &az, &gx, &gy, &gz, &temp) == 0) {
            printf("Ax:%.3f Ay:%.3f Az:%.3f | Gx:%.1f Gy:%.1f Gz:%.1f | T:%.1f°C\n",
                   ax, ay, az, gx, gy, gz, temp);
			printf("ax: %.3f\n",MPU6050_getAx_g(&mpu));
        }
        delay_ms(100);
    }
*/

